/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-03-01     86188       the first version
 */
#include "head_user.h"

//static enum
//{
//    pos = 0,
//    vel = 1,
//};


//
//static float Cov_X0[2][2], Cov_X1[2][2], \
//             X_00[2], X_01[2], X_1[2], P_00[2][2], P_01[2][2], P_1[2][2],\
//             F[2][2], F_T[2][2], B[2], a, Q[2][2], R[2][2], H[2][2], H_T[2][2], Z[2], K[2];
//
//static void MatPlus21(float (*OMat), const float (*LMat), const float (*RMat))
//{
//    OMat[0] = LMat[0] + RMat[0];
//    OMat[1] = LMat[1] + RMat[1];
//}
//static void MatPlus22(float (*OMat), const float (*LMat), const float (*RMat))
//{
//    OMat[0][0] = LMat[0][0] + RMat[0][0];
//    OMat[0][1] = LMat[0][1] + RMat[0][1];
//    OMat[1][0] = LMat[1][0] + RMat[1][0];
//    OMat[1][1] = LMat[1][1] + RMat[1][1];
//}
//static void MatMult22_21(float (*OMat), const float (*LMat)[2], const float (*RMat))
//{
//    OMat[0] = LMat[0][0]*RMat[0] + LMat[0][1]*RMat[1];
//    OMat[1] = LMat[1][0]*RMat[0] + LMat[1][1]*RMat[1];
//}
//static void MatMult21_1(float (*OMat), const float (*LMat), const float (*RMat))
//{
//    OMat[0] = LMat[0]*RMat[0];
//    OMat[1] = LMat[1]*RMat[0];
//}
//static void MatMult22_22(float (*OMat)[2], const float (*LMat)[2], const float (*RMat)[2])
//{
//    OMat[0][0] = LMat[0][0]*RMat[0][0] + LMat[0][1]*RMat[1][0];
//    OMat[0][1] = LMat[0][0]*RMat[0][1] + LMat[0][1]*RMat[1][1];
//    OMat[1][0] = LMat[1][0]*RMat[0][0] + LMat[1][1]*RMat[1][0];
//    OMat[1][1] = LMat[1][0]*RMat[0][1] + LMat[1][1]*RMat[1][1];
//}
//static void MatMult33_33(float (*OMat)[3], const float (*LMat)[3], const float (*RMat)[3])
//{
//    for (uint8_t i=0; i<3; i++)
//    {
//        for(uint8_t j=0; j<3; j++)
//        {
//            OMat[i][j] = 0;
//            for(uint8_t k=0; k<3; k++)
//            {
//                OMat[i][j] += a[i][k]*b[k][j] ;
//            }
//        }
//    }
//}
//static _ekf_measure(void)
//{
//    X_0[pos] = 0;
//    X_0[vel] = 0;
//}
//
//static _ekf_predict(float dt)
//{
//    /* 初始化矩阵 */
//
//    /* 状态转移:状态 */
//    MatMult22_21(X_01, F, X_00);
//    MatMult21_1(B, B, a);
//    MatPlus21(X_01, X_01, B);
//    /* 状态转移:协方差 */
//    MatMult22_22(P_01, F, P_00);
//    MatMult22_22(P_01, P_01, F_T);
//    MatPlus22(P_01, P_01, Q);
//}
//static _ekf_update(void)
//{
//    float tmp1[2][2], tmp2[2][2], tmp3[2][2];
//    /* 初始化矩阵 */
//
//    /* 状态更新:状态 */
//    MatMult22_22(tmp1, P_01, H_T);  //  P(k|k-1)*H'
//    MatMult22_22(tmp2, H, tmp1);    //H*P(k|k-1)*H'
//    MatMult22_22(tmp3, tmp2, R);    //H*P(k|k-1)*H' + R
//    //完整的要求矩阵的逆
//    K[0] = tmp1
//}
//static _ekf_estimate(void)
//{
//
//}


 /**********************************************************************
 * 函数名：卡尔曼滤波初始化
 * 功  能：  初始化滤波器起始参数
 * 输  入：  R：观测数据方差（噪声）数据质量越差此值越高，Q过程噪声，模型预测不到的随机噪声（如刮风，IMU温漂）
 *：             Cov：最优估计出的数据的方差，协方差。
 * 说  明：
 */
void EKF_init(EKF_HandleTypedef *ekf, float R, float *Cov, float *Q)
{
    for(uint8_t i=0; i<4; i++)
    {
        ekf->Q[i] = Q[i];
        ekf->Pre_cov[i] = Cov[i];
    }
    ekf->R[0] = R;
}

void  EKF_Update(EKF_HandleTypedef *ekf,    //惯性导航结构体
                 float Observation,         //位置观测量 cm
                 float acce,                //系统原始驱动量,惯导加速度 cm/s^2
                 float dt,                  //系统积分步长
                 uint8_t Axis)              //坐标系
{
    uint8_t i = 0;
    double Temp_cov[4]={0},k[2]={0};
    if(Axis =='X') i=0;
    if(Axis =='Y') i=1;
    if(Axis =='Z') i=2;

    ekf->Acce.axis[i]      = ekf->Acce_Bias.axis[i] + acce;
    ekf->Speed.axis[i]    += ekf->Acce.axis[i]*dt;
    ekf->Position.axis[i] += ekf->Speed.axis[i]*dt + (ekf->Acce.axis[i]*dt*dt)/2.0f;

    Temp_cov[0] = ekf->Pre_cov[0] + ekf->Pre_cov[2]*dt + (ekf->Pre_cov[1] + ekf->Pre_cov[3]*dt)*dt + ekf->Q[0];
    Temp_cov[1] = ekf->Pre_cov[1] + ekf->Pre_cov[3]*dt                                             + ekf->Q[1];
    Temp_cov[2] = ekf->Pre_cov[2] + ekf->Pre_cov[3]*dt                                             + ekf->Q[2];
    Temp_cov[3] = ekf->Pre_cov[3]                                                                  + ekf->Q[3];


    k[0]=Temp_cov[0] / (Temp_cov[0] + ekf->R[0]);
    k[1]=Temp_cov[2] / (Temp_cov[0] + ekf->R[0]);


    float Err = Observation - ekf->Position.axis[i];

    ekf->Speed.axis[i]      = ekf->Speed.axis[i]    + k[1]*Err;
    ekf->Position.axis[i]   = ekf->Position.axis[i] + k[0]*Err;
    ekf->Acce_Bias.axis[i] += 0.0002f*limit(k[1]*Err, -100, 100);
    ekf->Acce_Bias.axis[i]  = limit(ekf->Acce_Bias.axis[i], -10, +10);

    //更新状态协方差矩阵
    //Pre_cov=(I-K*H)*Temp_cov;  I为单位矩阵
    ekf->Pre_cov[0] = (1-k[0])*Temp_cov[0];
    ekf->Pre_cov[1] = (1-k[0])*Temp_cov[1];
    ekf->Pre_cov[2] = Temp_cov[2] - k[1]*Temp_cov[0];
    ekf->Pre_cov[3] = Temp_cov[3] - k[1]*Temp_cov[1];
}
